﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using GTN;

namespace ModuleBase.googoltech.GTN
{
    public static class GTNCard
    {
        private static short core = 1;
        public static bool Init(short _core, short axisCount, string cfgFileName, int ioSlave, int inputoffset = 2, int outputoffset = 8)
        {
            core = _core;
            try
            {
                //初始化控制器
                //打开控制器
                short sRtn = mc.GTN_Open(5, 1);//5:pci-e 2:保留
                if (sRtn != 0)
                {
                    return false;
                }
                //初始化EtherCAT总线，将调试软件生成的Gecat.xml文件放到程序的debug下和gts.dll同一层目录
                //返回值0，初始化成功；返回值-8，配置错误，检查ENI文件和实际连接从站是否匹配；
                //-9未找到ENI文件，将ENI问件放到可执行程序相同目录；-10未找到从站，检查接线
                sRtn = mc.GTN_InitEcatComm(core);
                if (sRtn != 0)
                {
                    return false;
                }
                short eCATStatus;//ecat通讯状态
                // 等待EtherCAT总线初始化成功
                do
                {
                    // 读取EtherCAT总线状态
                    sRtn = mc.GTN_IsEcatReady(core, out eCATStatus);
                } while (eCATStatus != 1);
                //启动EtherCAT通讯
                sRtn = mc.GTN_StartEcatComm(core);
                if (sRtn != 0)
                {
                    return false;
                }
                Thread.Sleep(500);
                // 卡复位
                sRtn = mc.GTN_Reset(core);//复位
                if (sRtn != 0)
                {
                    return false;
                }
                sRtn = mc.GTN_LoadConfig(core, cfgFileName);
                if (sRtn != 0)
                {
                    return false;
                }
                sRtn = mc.GTN_ClrSts(core, 1, axisCount);//16轴
                if (sRtn != 0)
                {
                    return false;
                }
                //设置轴到位误差带。
                for (int i = 0; i < 16 && i < axisCount; i++)
                {
                    sRtn = mc.GTN_SetAxisBand(core, (short)(i + 1), 50, 2);
                }
                sRtn = mc.GTN_EcatIOSynch(core);
                if (sRtn != 0)
                {
                    return false;
                }
                sRtn = mc.GTN_SetDo(core, mc.MC_GPO, 0x0000);//输出OFF
                for (int i = 0; i < 8; i++)
                {
                    sRtn = mc.GTN_RelateEcatSlaveToMcGpoBit(core, (short)(i + 1), (ushort)ioSlave, 0, (short)i, (short)outputoffset);
                    if (sRtn != 0)
                    {
                        return false;
                    }
                    sRtn = mc.GTN_RelateEcatSlaveToMcGpiBit(core, (short)(i + 1), (short)ioSlave, 0, (short)i, (short)inputoffset);
                    if (sRtn != 0)
                    {
                        return false;
                    }
                }
                for (int i = 0; i < 8; i++)
                {
                    sRtn = mc.GTN_RelateEcatSlaveToMcGpoBit(core, (short)(i + 1 + 8), (ushort)ioSlave, 0, (short)i, (short)(outputoffset + 1));
                    if (sRtn != 0)
                    {
                        return false;
                    }
                    sRtn = mc.GTN_RelateEcatSlaveToMcGpiBit(core, (short)(i + 1 + 8), (short)ioSlave, 0, (short)i, (short)(inputoffset + 1));
                    if (sRtn != 0)
                    {
                        return false;
                    }
                }

                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.Message);
                return false;
            }
        }
        public static void Close()
        {
            //先结束ecat通讯
            mc.GTN_TerminateEcatComm(core);

            //关闭控制卡
            mc.GTN_Close();
        }
        public static AxisStatus GetAxisStatus(AxisParm axisParm)
        {
            int AxisStatus = 0;
            uint temp_pClock = 0;
            mc.GTN_GetSts(axisParm.CardNo, axisParm.AxisId, out AxisStatus, 1, out temp_pClock);
            int pValue;
            AxisStatus result = default;
            result.FlagServoOn = (AxisStatus & 0x200) == 0x200;
            result.FlagAlm = (AxisStatus & 0x2) == 0x2;
            result.FlagPosLimit = (AxisStatus & 0x20) == 0x20;
            result.FlagNeglimit = (AxisStatus & 0x40) == 0x40;
            result.FlagMoveEnd = (AxisStatus & 0x400) != 0x400;
            result.FlagEmgStop = (AxisStatus & 0x100) == 0x100;
            result.FlagSmoothStop = (AxisStatus & 0x80) == 0x80;

            mc.GTN_GetDi(axisParm.CardNo, mc.MC_HOME, out pValue);
            result.FlagHome = (pValue & 1 << axisParm.AxisId - 1) == 0;
            double pValue1;
            uint pClock;
            mc.GTN_GetPrfPos(axisParm.CardNo, axisParm.AxisId, out pValue1, 1, out pClock);
            result.PrfPos = pValue1 * axisParm.Equiv;
            mc.GTN_GetEncPos(axisParm.CardNo, axisParm.AxisId, out pValue1, 1, out pClock);
            result.EncPos = pValue1 * axisParm.Equiv;
            return result;
        }
        public static bool GetAxisAlarm(AxisParm axisParm)
        {
            int AxisStatus = 0;
            uint temp_pClock = 0;
            mc.GTN_GetSts(axisParm.CardNo, axisParm.AxisId, out AxisStatus, 1, out temp_pClock);
            return (AxisStatus & 0x2) == 0x2;
        }
        public static double GetPos(AxisParm axisParm)
        {
            double pValue1;
            uint pClock;
            mc.GTN_GetPrfPos(axisParm.CardNo, axisParm.AxisId, out pValue1, 1, out pClock);
            return pValue1 * axisParm.Equiv;
        }
        public static double GetEnc(AxisParm axisParm)
        {
            double pValue1;
            uint pClock;
            mc.GTN_GetEncPos(axisParm.CardNo, axisParm.AxisId, out pValue1, 1, out pClock);
            return pValue1 * axisParm.Equiv;
        }
        public static double GetEcatEnc(AxisParm axisParm)
        {
            var sRtn = mc.GTN_GetEcatEncPos(axisParm.CardNo, axisParm.AxisId, out var pEncPos);
            return pEncPos * axisParm.Equiv;
        }
        public static void ServoOn(AxisParm axisParm)
        {
            mc.GTN_AxisOn(axisParm.CardNo, axisParm.AxisId);
        }

        public static void ServoOff(AxisParm axisParm)
        {
            mc.GTN_AxisOff(axisParm.CardNo, axisParm.AxisId);
        }
        public static void ClearAlm(AxisParm axisParm)
        {
            mc.GTN_SetDoBit(axisParm.CardNo, mc.MC_CLEAR, axisParm.AxisId, 0);
            System.Threading.Thread.Sleep(1000);
            mc.GTN_SetDoBit(axisParm.CardNo, mc.MC_CLEAR, axisParm.AxisId, 1);
            mc.GTN_ClrSts(axisParm.CardNo, axisParm.AxisId, 1);
        }
        public static void ClearAlm1(AxisParm axisParm)
        {
            mc.GTN_ClrSts(axisParm.CardNo, axisParm.AxisId, 1);
        }
        public static bool GetDi(short cardNum, ushort bitno)
        {
            int pValue;
            mc.GTN_GetDi(cardNum, mc.MC_GPI, out pValue);
            return (pValue & 1 << bitno) == 0;
        }
        public static void SetDo(short cardNum, short outbit, short value)
        {
            mc.GTN_SetDoBit(cardNum, mc.MC_GPO, (short)(outbit + 1), value);
        }
        public static int GetDiPort1(short cardNum)
        {
            int pValue;
            mc.GTN_GetDi(cardNum, mc.MC_GPI, out pValue);
            return pValue;
        }
        public static int GetDoPort1(short cardNum)
        {
            int pValue;
            mc.GTN_GetDo(cardNum, mc.MC_GPO, out pValue);
            return pValue;
        }
        public static void SetDoPort1(short cardNum, int value)
        {
            mc.GTN_SetDo(cardNum, mc.MC_GPO, value);
        }
        public static void AxisHomeMove(AxisParm _axisParam)//通过驱动器回零
        {
            mc.GTN_SetHomingMode(_axisParam.CardNo, _axisParam.AxisId, 6);//切换到回零模式
            //回零速度:指令单位/s
            //加速度:指令单位/s²
            mc.GTN_SetEcatHomingPrm(_axisParam.CardNo, _axisParam.AxisId, _axisParam.HomeMode,
                _axisParam.HomeHSpd / _axisParam.Equiv, _axisParam.HomeLSpd / _axisParam.Equiv, _axisParam.HomeHSpd / _axisParam.Equiv * 10, 0, 0);
            mc.GTN_StartEcatHoming(_axisParam.CardNo, _axisParam.AxisId);//启动 Smart Home 回原点
        }
        public static void AxisHomeStop(AxisParm _axisParam)
        {
            mc.GTN_StopEcatHoming(_axisParam.CardNo, _axisParam.AxisId);
            Thread.Sleep(100);
            mc.GTN_SetHomingMode(_axisParam.CardNo, _axisParam.AxisId, 8);//切换到位置控制模式
        }
        public static bool AxisHomeCheckDone(AxisParm _axisParam)
        {
            mc.GTN_GetEcatHomingStatus(_axisParam.CardNo, _axisParam.AxisId, out var homingStatus);
            if (homingStatus == 3)
            {
                mc.GTN_SetHomingMode(_axisParam.CardNo, _axisParam.AxisId, 8);//切换到位置控制模式
                return true;
            }
            else
            {
                return false;
            }
        }
        public static void AxisZeroSet(AxisParm _axisParam)
        {
            mc.GTN_ZeroPos(_axisParam.CardNo, _axisParam.AxisId, 1);//清零规划位置和实际位置，并进行零漂补偿。
        }

        public static void AxisPosSet(AxisParm _axisParam, double value)
        {
            mc.GTN_SetPrfPos(_axisParam.CardNo, _axisParam.AxisId, (int)Math.Round(value / _axisParam.Equiv, 0));
        }

        public static void AxisEncSet(AxisParm _axisParam, double value)
        {
            mc.GTN_SetEncPos(_axisParam.CardNo, _axisParam.AxisId, (int)Math.Round(value / _axisParam.Equiv, 0));
        }
        public static void AxisSynchAxisPos(AxisParm _axisParam)
        {
            mc.GTN_SynchAxisPos(_axisParam.CardNo, 1 << (_axisParam.AxisId - 1));
        }
        public static void AxisPosMove(ref AxisParm _AxisParam, double givePos, double speed = 0, double acc = 0)
        {
            _AxisParam.Target = givePos / _AxisParam.Equiv;
            mc.TTrapPrm ATrapPrm = new mc.TTrapPrm();
            mc.GTN_PrfTrap(_AxisParam.CardNo, _AxisParam.AxisId);
            mc.GTN_GetTrapPrm(_AxisParam.CardNo, _AxisParam.AxisId, out ATrapPrm); //读取点位模式运动参数
            if (acc == 0)
            {
                ATrapPrm.acc = _AxisParam.Acc;
                ATrapPrm.dec = _AxisParam.Acc;
            }
            else
            {
                ATrapPrm.acc = acc;
                ATrapPrm.dec = acc;
            }
            ATrapPrm.smoothTime = 25;
            mc.GTN_SetTrapPrm(_AxisParam.CardNo, _AxisParam.AxisId, ref ATrapPrm); //设置点位模式运动参数
            mc.GTN_SetPos(_AxisParam.CardNo, _AxisParam.AxisId, (int)Math.Round(givePos / _AxisParam.Equiv, 0)); //设置目标位置
            double max_Vel;
            if (speed == 0.0)
            {
                max_Vel = _AxisParam.MaxWorkSpd / _AxisParam.Equiv / 1000;
            }
            else
            {
                max_Vel = speed / _AxisParam.Equiv / 1000;
            }
            mc.GTN_SetVel(_AxisParam.CardNo, _AxisParam.AxisId, max_Vel); //设置目标速度
            mc.GTN_Update(_AxisParam.CardNo, 1 << _AxisParam.AxisId - 1); //启动当前轴运动
        }

        public static bool AxisPosMoveCheckDone(AxisParm _AxisParam)
        {
            int AxisStatus = 0;
            uint temp_pClock = 0;
            mc.GTN_GetSts(_AxisParam.CardNo, _AxisParam.AxisId, out AxisStatus, 1, out temp_pClock);
            if ((AxisStatus & 0x400) != 0x400)
            {
                double pValue1;
                uint pClock;
                mc.GTN_GetEncPos(_AxisParam.CardNo, _AxisParam.AxisId, out pValue1, 1, out pClock);
                if (Math.Abs(pValue1 - _AxisParam.Target) < 100)
                {
                    return true;
                }
            }
            return false;
        }
        public static void AxisJog(AxisParm _AxisParam, ushort dir, double spd = 0)
        {
            double Vel_JSpeed = 0;
            if (dir == 0)//反向
            {
                Vel_JSpeed = spd / _AxisParam.Equiv / 1000 * -1;
            }
            else
            {
                Vel_JSpeed = spd / _AxisParam.Equiv / 1000;
            }
            mc.TJogPrm JogPrm = new mc.TJogPrm();
            mc.GTN_PrfJog(_AxisParam.CardNo, _AxisParam.AxisId);
            mc.GTN_GetJogPrm(_AxisParam.CardNo, _AxisParam.AxisId, out JogPrm);
            JogPrm.acc = 1;
            JogPrm.dec = 1;
            mc.GTN_SetJogPrm(_AxisParam.CardNo, _AxisParam.AxisId, ref JogPrm);
            mc.GTN_SetVel(_AxisParam.CardNo, _AxisParam.AxisId, Vel_JSpeed); //设置当前轴的目标速度
            mc.GTN_Update(_AxisParam.CardNo, 1 << _AxisParam.AxisId - 1); //启动当前轴运动
        }

        public static void AxisStop(AxisParm _AxisParam, int type)
        {
            mc.GTN_Stop(_AxisParam.CardNo, 1 << _AxisParam.AxisId - 1, type << _AxisParam.AxisId - 1);
        }
        public static bool SetCrd3D(short cardNum, short crd, bool isAbs, short x, short y, short z, double maxSpeed, double maxAcc)
        {
            try
            {
                mc.TCrdPrm crdPrm;

                crdPrm.dimension = 3;                        // 建立二维的坐标系
                crdPrm.synVelMax = maxSpeed;                      // 坐标系的最大合成速度是: 500 pulse/ms
                crdPrm.synAccMax = maxAcc;                        // 坐标系的最大合成加速度是: 2 pulse/ms^2
                crdPrm.evenTime = 0;                         // 坐标系的最小匀速时间为0
                short[] axisArr = new short[8];
                //找X
                axisArr[x - 1] = 1;
                //找Y
                axisArr[y - 1] = 2;
                //找Z
                axisArr[z - 1] = 3;
                crdPrm.profile1 = axisArr[0];                       // 规划器1对应到X轴                       
                crdPrm.profile2 = axisArr[1];                       // 规划器2对应到Y轴
                crdPrm.profile3 = axisArr[2];
                crdPrm.profile4 = axisArr[3];
                crdPrm.profile5 = axisArr[4];
                crdPrm.profile6 = axisArr[5];
                crdPrm.profile7 = axisArr[6];
                crdPrm.profile8 = axisArr[7];
                crdPrm.setOriginFlag = (short)(isAbs ? 1 : 0);                    // 需要设置加工坐标系原点位置
                crdPrm.originPos1 = 0;                     // 加工坐标系原点位置在(0,0,0)，即与机床坐标系原点重合
                crdPrm.originPos2 = 0;
                crdPrm.originPos3 = 0;
                crdPrm.originPos4 = 0;
                crdPrm.originPos5 = 0;
                crdPrm.originPos6 = 0;
                crdPrm.originPos7 = 0;
                crdPrm.originPos8 = 0;

                short SRtn = mc.GTN_SetCrdPrm(cardNum, crd, ref crdPrm);
                if (SRtn != 0)
                {
                    Debug.WriteLine($"GTN_SetCrdPrm:{SRtn}");
                    return false;
                }

                mc.TCrdData[] crdData = new mc.TCrdData[200];

                SRtn = mc.GTN_InitLookAhead(cardNum, crd, 0, 10, maxAcc, 200, ref crdData[0]);
                if (SRtn != 0)
                {
                    Debug.WriteLine($"GTN_InitLookAhead:{SRtn}");
                    return false;
                }

                return true;
            }
            catch (Exception ex)
            {
                Debug.WriteLine($"SetCrd3D:{ex.Message}");
                return false;
            }
        }
        public static void AxisLnXYMove3D(AxisParm XAxis, AxisParm YAxis, AxisParm ZAxis, List<M1Point> targets, short crd, double acc)
        {
            // 指令返回值
            short sRtn;
            int space;
            sRtn = mc.GTN_CrdClear(XAxis.CardNo, crd, 0);
            for (int i = 0; i < targets.Count; i++)
            {
                double _acc = acc <= 0 ? targets[i].Acc : acc;
                switch (targets[i].Type)
                {
                    case 0://直线普通定位
                        sRtn = mc.GTN_LnXYZ(XAxis.CardNo, crd, (int)(targets[i].X / XAxis.Equiv), (int)(targets[i].Y / YAxis.Equiv), (int)(targets[i].Z / ZAxis.Equiv), targets[i].Speed / XAxis.Equiv / 1000, _acc, 0, 0);
                        break;
                    case 1://直线精确定位
                        sRtn = mc.GTN_LnXYZG0(XAxis.CardNo, crd, (int)(targets[i].X / XAxis.Equiv), (int)(targets[i].Y / YAxis.Equiv), (int)(targets[i].Z / ZAxis.Equiv), targets[i].Speed / XAxis.Equiv / 1000, _acc, 0);
                        break;
                    case 2://圆弧定位
                        sRtn = mc.GTN_ArcXYZ(XAxis.CardNo, crd, (int)(targets[i].X / XAxis.Equiv), (int)(targets[i].Y / YAxis.Equiv), (int)(targets[i].Z / ZAxis.Equiv),
                           (int)(targets[i].MidX / XAxis.Equiv), (int)(targets[i].MidY / YAxis.Equiv), (int)(targets[i].MidZ / ZAxis.Equiv),
                           targets[i].Speed / XAxis.Equiv / 1000, _acc, 0, 0);
                        break;
                    case 3://IO操作
                        sRtn = mc.GTN_BufIO(XAxis.CardNo, crd, mc.MC_GPO, (ushort)(1 << targets[i].IOIndex), (ushort)((targets[i].OnOff ? 0 : 1) << targets[i].IOIndex), 0);
                        break;
                    case 5://延时
                        sRtn = mc.GTN_BufDelay(XAxis.CardNo, crd, (ushort)(targets[i].Delay * 1000), 0);
                        break;
                    default:
                        break;
                }
            }
            // 将前瞻缓存区中的数据压入控制器
            sRtn = mc.GTN_CrdData(XAxis.CardNo, crd, IntPtr.Zero, 0);
        }
        public static void AxisStartCrd(short cardNum, short crd)
        {
            mc.GTN_CrdStart(cardNum, (short)(1 << crd - 1), 0);
        }
        /// <summary>
        /// 坐标系停止
        /// </summary>
        /// <param name="cardNum"></param>
        /// <param name="crdIndex">坐标系</param>
        /// <param name="type"></param>
        //public static void AxisStop(short cardNum, int crdIndex, int type)
        //{
        //    mc.GTN_Stop(cardNum, 1 << 8 + crdIndex - 1, type << 8 + crdIndex - 1);
        //}
        public static bool AxisCheckCrdDone(short cardNum, short crd)
        {
            short run;
            int seg;
            mc.GTN_CrdStatus(cardNum, crd, out run, out seg, 0);
            return run != 1;
        }
        public static void AxisPTMove(ref AxisParm _AxisParam, PTData[] data, short fifo)
        {
            double pos = 0;
            int time = 0;
            var r1 = mc.GTN_PrfPt(_AxisParam.CardNo, _AxisParam.AxisId, mc.PT_MODE_STATIC);//设置静态PT运动模式
            r1 = mc.GTN_PtClear(_AxisParam.CardNo, _AxisParam.AxisId, fifo);//清空FIFO0
            for (int i = 0; i < data.Length; i++)
            {
                pos += (int)Math.Round(data[i].pos / _AxisParam.Equiv, 0);
                time += data[i].time;
                short type1 = mc.PT_SEGMENT_NORMAL;
                if (i == data.Length - 1)
                {
                    type1 = mc.PT_SEGMENT_STOP;
                }
                r1 = mc.GTN_PtData(_AxisParam.CardNo, _AxisParam.AxisId, pos, time, type1, fifo);
            }
            double pValue1;
            uint pClock;
            mc.GTN_GetEncPos(_AxisParam.CardNo, _AxisParam.AxisId, out pValue1, 1, out pClock);
            _AxisParam.Target = pValue1 + pos;
            int option = fifo;
            r1 = mc.GTN_PtStart(_AxisParam.CardNo, 1 << _AxisParam.AxisId - 1, option << _AxisParam.AxisId - 1);//启动FIFO的PT运动
        }
        public static short AxisPtSpace(AxisParm _AxisParam, short fifo)
        {
            mc.GTN_PtSpace(_AxisParam.CardNo, _AxisParam.AxisId, out var pSpace, fifo);
            return pSpace;
        }
        public static void SetSoftLimit(AxisParm _AxisParam)
        {
            mc.GTN_SetSoftLimit(_AxisParam.CardNo, _AxisParam.AxisId, (int)(_AxisParam.PosLimit / _AxisParam.Equiv), (int)(_AxisParam.NegLimit / _AxisParam.Equiv));
        }
        public static int GetLimitStatus(AxisParm _AxisParam)
        {
            mc.GTN_GetLimitStatus(_AxisParam.CardNo, _AxisParam.AxisId, out var pLimitPos, out var pLimitNeg);
            return (pLimitPos != 0 ? 1 : 0) << 1 + (pLimitNeg != 0 ? 1 : 0);
        }
    }
}
